/*!
 * Copyright (c) 2012
 * Automation and Robotics Lab (LARA) at University of Brasilia (UnB)
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     - Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     - Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     - Neither the name of the Automation and Robotics Lab (LARA) nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 3 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License LGPL for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL along with this program.
 * If not, see <http://www.gnu.org/licenses/>.
 *
 *****************************************************************
 *
 *   ROS stack name: schunk
 *   ROS package name: schunk_low
 *
 *   Authors: André A. Geraldes, email: andregeraldes@lara.unb.br
 *            Murilo M. Marinho, email: murilomarinho@lara.unb.br
 *
 *****************************************************************/

/******************************************************************************
                                   INCLUDES
 *****************************************************************************/

// schunk_low
#include <schunk_low/schunk.h>

// General
#include <string>
#include <vector>

// ROS
#include <ros/ros.h>

// Library for communicating with the hardware
#include <schunk_libm5api/m5apiw32.h>

// Namespace std
using namespace std;

/******************************************************************************
                          CONSTRUCTORS AND DESTRUCTORS
 *****************************************************************************/

Schunk::Schunk(ros::NodeHandle n)
{
    pthread_mutex_init(&mutex_, NULL);
    can_parameters_set_ = false;
    robot_parameters_set_ = false;
    pid_parameters_set_ = false;
    initialized_ = false;
    can_device_opened_ = false;
}

Schunk::~Schunk()
{
      if (can_device_opened_)
      {
          pthread_mutex_lock(&mutex_);
          PCube_closeDevice(device_);
          pthread_mutex_unlock(&mutex_);
      }
}

/******************************************************************************
                      METHODS FOR SETTING MEMEBER VARIABLES
 *****************************************************************************/

bool Schunk::SetCanParameters(std::string can_module, std::string can_device, int can_baudrate)
{
    can_module_ = can_module;
    can_device_ = can_device;
    can_baudrate_ = can_baudrate;

    can_parameters_set_ = true;

    return true;
}

bool Schunk::SetRobotParameters(const int& module_count, const std::vector<int>& module_ids, const std::vector<std::string>& module_names,
            const std::vector<double>& max_positions, const std::vector<double>& min_positions,
            const std::vector<double>& home_offsets, const std::vector<double>& home_positions, const std::vector<double>& home_velocities,
            const std::vector<double>& max_velocities, const std::vector<double>& max_accelerations)
{
    module_count_ = module_count;
    module_ids_ = module_ids;
    module_names_ = module_names;
    max_positions_ = max_positions;
    min_positions_ = min_positions;
    home_offsets_ = home_offsets;
    home_positions_ = home_positions;
    home_velocities_ = home_velocities;
    max_velocities_ = max_velocities;
    max_accelerations_ = max_accelerations;

    robot_parameters_set_ = true;

    return true;
}

bool Schunk::SetPIDParameters(const std::vector<int>& A0, const std::vector<int>& C0, const std::vector<int>& Damp)
{
    A0_ = A0;
    C0_ = C0;
    Damp_ = Damp;

    pid_parameters_set_ = true;

    return true;
}

bool Schunk::set_max_positions(const std::vector<double>& max_positions)
{
    max_positions_ = max_positions;
    return send_max_positions();
}

bool Schunk::set_min_positions(const std::vector<double>& min_positions)
{
    min_positions_ = min_positions;
    return send_min_positions();
}

bool Schunk::set_home_offsets(const std::vector<double>& home_offsets)
{
    home_offsets_ = home_offsets;
    return send_home_offsets();
}

bool Schunk::set_home_positions(const std::vector<double>& home_positions)
{
    home_positions_ = home_positions;
    return true;
}

bool Schunk::set_home_velocities(const std::vector<double>& home_velocities)
{
    home_velocities_ = home_velocities;
    return send_home_velocities();
}

bool Schunk::set_max_velocities(const std::vector<double>& max_velocities)
{
    max_velocities_ = max_velocities;
    return send_max_velocities();
}

bool Schunk::set_max_accelerations(const std::vector<double>& max_accelerations)
{
    max_accelerations_ = max_accelerations;
    return send_max_accelerations();
}

/******************************************************************************
                       METHODS FOR READING ROBOT'S STATE
 *****************************************************************************/

void Schunk::GetCurrentPositions(std::vector<double> *positions)
{
    float position;
    for (int i = 0; i < module_count_; i++)
    {
        pthread_mutex_lock(&mutex_);
        PCube_getPos(device_, module_ids_[i], &position);
        (*positions)[i] = static_cast<double>(position);
        pthread_mutex_unlock(&mutex_);
    }
}

std::vector<double> Schunk::GetCurrentPositions()
{
    std::vector<double> positions;
    positions.resize(module_count_);

    float position;
    for (int i = 0; i < module_count_; i++)
    {
        pthread_mutex_lock(&mutex_);
        PCube_getPos(device_, module_ids_[i], &position);
        positions[i] = static_cast<double>(position);
        pthread_mutex_unlock(&mutex_);
    }

    return positions;
}

void Schunk::GetCurrentVelocities(std::vector<double> *velocities)
{
    float velocity;
    for (int i = 0; i < module_count_; i++)
    {
        pthread_mutex_lock(&mutex_);
        PCube_getVel(device_, module_ids_[i], &velocity);
        (*velocities)[i] = static_cast<double>(velocity);
        pthread_mutex_unlock(&mutex_);
    }
}

std::vector<double> Schunk::GetCurrentVelocities()
{
    std::vector<double> velocities;
    velocities.resize(module_count_);

    float velocity;
    for (int i = 0; i < module_count_; i++)
    {
        pthread_mutex_lock(&mutex_);
        PCube_getVel(device_, module_ids_[i], &velocity);
        velocities[i] = static_cast<double>(velocity);
        pthread_mutex_unlock(&mutex_);
    }

    return velocities;
}

std::vector<int> Schunk::GetCurrentA0()
{
    std::vector<int> A0s;
    A0s.resize(module_count_);

    short A0;
    for (int i = 0; i < module_count_; i++)
    {
        pthread_mutex_lock(&mutex_);
        PCube_getA0(device_, module_ids_[i],  &A0);
        A0s[i] = static_cast<int>(A0);
        pthread_mutex_unlock(&mutex_);
    }

    return A0s;
}

std::vector<int> Schunk::GetCurrentC0()
{
    std::vector<int> C0s;
    C0s.resize(module_count_);

    short C0;
    for (int i = 0; i < module_count_; i++)
    {
        pthread_mutex_lock(&mutex_);
        PCube_getC0(device_, module_ids_[i],  &C0);
        C0s[i] = static_cast<int>(C0);
        pthread_mutex_unlock(&mutex_);
    }

    return C0s;
}

std::vector<int> Schunk::GetCurrentDamp()
{
    std::vector<int> Damps;
    Damps.resize(module_count_);

    short Damp;
    for (int i = 0; i < module_count_; i++)
    {
        pthread_mutex_lock(&mutex_);
        PCube_getDamp(device_, module_ids_[i],  &Damp);
        Damps[i] = static_cast<int>(Damp);
        pthread_mutex_unlock(&mutex_);
    }

    return Damps;
}

std::vector<int> Schunk::GetCurrentStates()
{
    std::vector<int> states;
    states.resize(module_count_);

    unsigned long state;
    for (int i = 0; i < module_count_; i++)
    {
        pthread_mutex_lock(&mutex_);
        PCube_getModuleState(device_, module_ids_[i], &state);
        states[i] = static_cast<int>(state);
        pthread_mutex_unlock(&mutex_);
    }

    return states;
}

/******************************************************************************
             METHODS FOR INITIALIZING AND CONFIGURING THE ROBOT
 *****************************************************************************/

bool Schunk::Init()
{
    // If robot is already initialized, there is nothing left to do
    if (initialized_)
    {
        ROS_INFO_STREAM("Schunk::Init - Object already initialized");
        return true;
    }

    // Robot can only be initialized if parameters have already been set
    if (!can_parameters_set_ || !robot_parameters_set_ || !pid_parameters_set_)
    {
        ROS_ERROR_STREAM("Schunk::Init - Cannot initialize object! Parameters have not been set yet");
        return false;
    }

    // Display robot's parameters in console
    ROS_INFO_STREAM("=========================================================================== ");
    ROS_INFO_STREAM("Schunk->init(): Trying to initialize with the following parameters: ");
    PrintParameters();
    ROS_INFO_STREAM("=========================================================================== ");

    // Open CAN device_
    if (!OpenDevice())
    {
        ROS_ERROR_STREAM("Schunk::Init - Error opening the CAN device_. Cannot initialize schunk object");
        return false;
    }

    // Reset modules and check if they are connected to the bus
    ResetAllModules();
    if (!ProbeModules())
    {
        ROS_ERROR_STREAM("Schunk::Init - Error probing the modules. Cannot initialize schunk object");
        return false;
    }

    // Send parameters to the hardware
    SendPIDParameters();
    send_max_positions();
    send_min_positions();
    send_home_offsets();
    send_home_velocities();
    send_max_velocities();
    send_max_accelerations();

    // Set synchronous or asynchronous movements
    SetASyncMotion();

    // Init messages
    InitMessages();

    // All modules initialized successfully
    initialized_ = true;

    return true;
}

bool Schunk::InitGripper()
{
    int gripper_index = module_count_-1;
    int gripper_id = module_ids_[gripper_index];

    std::vector<int> states = GetCurrentStates();
    int gripper_state = states[gripper_index];

    Reset(gripper_id);

    // Check if the bit 2 of the status word (MOD HOME is set)
    if ((gripper_state >> 1) % 2 == 0)
    {
        // If the bit 2 of the status word is not set, the module needs to be homed
        DoHoming(gripper_id);

        states = GetCurrentStates();
        gripper_state = states[gripper_index];
        if ((gripper_state >> 1) % 2 == 0)
        {
            ROS_INFO_STREAM("Schunk::InitGripper - Unable to home the gripper");
            return false;
        }
    }

    Reset(gripper_id);

    return true;
}

bool Schunk::Close()
{
    if (can_device_opened_)
    {
        initialized_ = false;
        can_device_opened_ = false;

        pthread_mutex_lock(&mutex_);
        PCube_closeDevice(device_);
        pthread_mutex_unlock(&mutex_);

        ROS_INFO_STREAM("Schunk::Close - Can device closed successfully");
        return true;
    }

    else
    {
        ROS_INFO_STREAM("Schunk::Close - Can device was already closed");
        return false;
    }
}

bool Schunk::ResetAllModules()
{
    int ret = 0;

    pthread_mutex_lock(&mutex_);
    ret = PCube_resetAll(device_);
    pthread_mutex_unlock(&mutex_);

    if (ret != 0)
    {
        ROS_ERROR_STREAM("Schunk::ResetAllModules - Could not reset all modules, m5api error code: " << ret);
        return false;
    }

    return true;
}

bool Schunk::ResetModule(int module_id)
{
    int ret = 0;

    pthread_mutex_lock(&mutex_);
    ret = PCube_resetModule(device_, module_id);
    pthread_mutex_unlock(&mutex_);

    if (ret != 0)
    {
        ROS_ERROR_STREAM("Schunk::ResetModule - Could not reset module, m5api error code: " << ret);
        return false;
    }

    return true;
}

bool Schunk::SetSyncMotion()
{
    if (can_device_opened_)
    {
        for (int i = 0; i < module_count_; i++)
        {
            unsigned long confword;

            pthread_mutex_lock(&mutex_);
            PCube_getConfig(device_, module_ids_[i], &confword);
            pthread_mutex_unlock(&mutex_);

            pthread_mutex_lock(&mutex_);
            PCube_setConfig(device_, module_ids_[i], confword | CONFIGID_MOD_SYNC_MOTION);
            pthread_mutex_unlock(&mutex_);
        }
        return true;
    }

    else
    {
        ROS_ERROR_STREAM("Schunk::SetSyncMotion - Module not initialized");
        return false;
    }
}

bool Schunk::SetASyncMotion()
{
    if (can_device_opened_)
    {
        for (int i = 0; i < module_count_; i++)
        {
            unsigned long confword;

            pthread_mutex_lock(&mutex_);
            PCube_getConfig(device_, module_ids_[i], &confword);
            pthread_mutex_unlock(&mutex_);

            pthread_mutex_lock(&mutex_);
            PCube_setConfig(device_, module_ids_[i], confword & (~CONFIGID_MOD_SYNC_MOTION));
            pthread_mutex_unlock(&mutex_);
        }
        return true;
    }

    else
    {
        ROS_ERROR_STREAM("Schunk::SetASyncMotion - Module not initialized");
        return false;
    }
}

/******************************************************************************
                        METHODS FOR MOVING THE ROBOT
 *****************************************************************************/

bool Schunk::Move()
{
    if (initialized_)
    {
        pthread_mutex_lock(&mutex_);
        PCube_startMotionAll(device_);
        pthread_mutex_unlock(&mutex_);
        return true;
    }

    else
    {
        ROS_ERROR_STREAM("Schunk::Move - Module not initialized");
        return false;
    }
}

bool Schunk::Stop()
{
    int ret = 0;

    pthread_mutex_lock(&mutex_);
    ret = PCube_haltAll(device_);
    pthread_mutex_unlock(&mutex_);

    if (ret != 0)
    {
        ROS_ERROR_STREAM("Schunk::Stop - Could not stop the robot: " << ret);
        return false;
    }

    return true;
}

bool Schunk::Reset()
{
    if (initialized_)
    {
        pthread_mutex_lock(&mutex_);
        PCube_resetAll(device_);
        pthread_mutex_unlock(&mutex_);
        return true;
    }

    else
    {
        ROS_ERROR_STREAM("Schunk::Reset - Module not initialized");
        return false;
    }
}

bool Schunk::Reset(int module_id)
{
    if (initialized_)
    {
        pthread_mutex_lock(&mutex_);
        PCube_resetModule(device_, module_id);
        pthread_mutex_unlock(&mutex_);
        return true;
    }

    else
    {
        ROS_ERROR_STREAM("Schunk::Reset - Module not initialized");
        return false;
    }
}

bool Schunk::DoHoming()
{
    if (initialized_)
    {
        pthread_mutex_lock(&mutex_);
        PCube_homeAll(device_);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_waitForHomeEndAll(device_, 60000000);
        pthread_mutex_unlock(&mutex_);

        return true;
    }

    else
    {
        ROS_ERROR_STREAM("Schunk::DoHoming - Module not initialized");
        return false;
    }
}

bool Schunk::DoHoming(int module_id)
{
    if (initialized_)
    {
        pthread_mutex_lock(&mutex_);
        PCube_homeModule(device_, module_id);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_waitForHomeEnd(device_, module_id, 60000000);
        pthread_mutex_unlock(&mutex_);

        return true;
    }

    else
    {
        ROS_ERROR_STREAM("Schunk::DoHoming - Module not initialized");
        return false;
    }
}

bool Schunk::DoHardHoming(int module_count)
{
    if (initialized_)
    {
        std::vector<double> hard_home_positions;
        std::vector<double> hard_home_velocities;
        std::vector<double> hard_home_accelerations;

        hard_home_positions.resize(module_count);
        hard_home_velocities.resize(module_count);
        hard_home_accelerations.resize(module_count);

        for (int i  = 0; i < module_count; i++)
        {
            hard_home_positions[i] = home_positions_[i];
            hard_home_velocities[i] = home_velocities_[i];
            hard_home_accelerations[i] = 0.1;
        }

        SetNextPosture(hard_home_positions, hard_home_velocities, hard_home_accelerations);
        Move();

        pthread_mutex_lock(&mutex_);
        PCube_waitForMotionEndAll(device_, 60000000);
        pthread_mutex_unlock(&mutex_);

        return true;
    }
    else
    {
        ROS_ERROR_STREAM("Schunk::DoHardHoming - Module not initialized");
        return false;
    }
}

bool Schunk::FinishedMoving()
{
    bool finished = true;
    unsigned long module_state;

    for (int i = 0; i < module_count_; i++)
    {
        pthread_mutex_lock(&mutex_);
        PCube_getModuleState(device_, module_ids_[i], &module_state);
        if ((module_state & STATEID_MOD_INPROGRESS) != 0)
        {
            finished = false;
        }
        pthread_mutex_unlock(&mutex_);
    }
    return finished;
}

void Schunk::SetNextPosture(std::vector<double> pos, std::vector<double> vel, std::vector<double> acc)
{
    if (initialized_)
    {
        if (pos.size() == vel.size() && pos.size() == acc.size() && static_cast<int>(pos.size()) <= module_count_)
        {
            for (int i = 0; i < module_count_; i++)
            {
                pthread_mutex_lock(&mutex_);
                PCube_moveRamp(device_, module_ids_[i], pos[i], vel[i], acc[i]);
                pthread_mutex_unlock(&mutex_);
            }
        }
    }
}

void Schunk::SetVelocitiesForNextPosture(std::vector<double> vel)
{
    if (initialized_)
    {
        if (static_cast<int>(vel.size()) <= module_count_)
        {
            for (int i = 0; i < static_cast<int>(vel.size()); i++)
            {
                pthread_mutex_lock(&mutex_);
                PCube_moveVel(device_, module_ids_[i], vel[i]);
                pthread_mutex_unlock(&mutex_);
            }
        }
    }
}

void Schunk::SetJointValueForNextMove(int module_id, double pos, double vel, double acc)
{
    if (initialized_)
    {
        pthread_mutex_lock(&mutex_);
        PCube_moveRamp(device_, module_id, pos, vel, acc);
        pthread_mutex_unlock(&mutex_);
    }
}

void Schunk::SetJointVelocityForNextMove(int module_id, double vel)
{
    if (initialized_)
    {
        pthread_mutex_lock(&mutex_);
        PCube_moveVel(device_, module_id, vel);
        pthread_mutex_unlock(&mutex_);
    }
}

/******************************************************************************
           METHODS FOR STORING ROBOT'S INFORMATION IN MESSAGES
 *****************************************************************************/
// schunk_msgs::AllParameters Schunk::readAllParameters();
// The function readAllParameters is defined in another file (schunk_readAllParameters.cpp)
// because its to big to be placed here

// schunk_msgs::JointStateComplete Schunk::readJointStates();
// The function readJointStates is defined in another file (schunk_readJointStates.cpp)
// because its to big to be placed here

// schunk_msgs::JointStateElectrical Schunk::readElectricalStates();
// The function readElectricalStates is defined in another file (schunk_readElectricalStates.cpp)
// because its to big to be placed here


/******************************************************************************
               METHODS FOR SENDING THE PARAMETERS TO THE HARDWARE
 *****************************************************************************/

void Schunk::SendPIDParameters()
{
    bool changed;

    for (int i = 0; i < module_count_; i++)
    {
        changed = false;

        // Set A0
        if (A0_[i] >= 1 && A0_[i] <= 4)
        {
            pthread_mutex_lock(&mutex_);
            PCube_setA0(device_, module_ids_[i], A0_[i]);
            pthread_mutex_unlock(&mutex_);
            changed = true;
        }

        // Set C0
        if (C0_[i] >= 28 && C0_[i] <= 64 && C0_[i]%2 == 0)
        {
            pthread_mutex_lock(&mutex_);
            PCube_setC0(device_, module_ids_[i], C0_[i]);
            pthread_mutex_unlock(&mutex_);
            changed = true;
        }

        // Set Damp
        if (Damp_[i] >= 1 && Damp_[i] <= 4)
        {
            pthread_mutex_lock(&mutex_);
            PCube_setDamp(device_, module_ids_[i], Damp_[i]);
            pthread_mutex_unlock(&mutex_);
            changed = true;
        }

        // Recalculate the PID Parameters if any has been modified
        if (changed)
        {
            pthread_mutex_lock(&mutex_);
            PCube_recalcPIDParams(device_, module_ids_[i]);
            pthread_mutex_unlock(&mutex_);
        }
    }
}

bool Schunk::send_max_positions()
{
    int ret = 0;

    for (int i = 0; i < module_count_; i++)
    {
        pthread_mutex_lock(&mutex_);
        ret = PCube_setMaxPos(device_, module_ids_[i], max_positions_[i]);
        pthread_mutex_unlock(&mutex_);
    }

    if (ret != 0)
    {
        ROS_ERROR_STREAM("Schunk::send_max_positions - Could not communicate with hardware. m5api error code: " << ret);
        return false;
    }
    return true;
}

bool Schunk::send_min_positions()
{
    int ret = 0;

    for (int i = 0; i < module_count_; i++)
    {
        pthread_mutex_lock(&mutex_);
        ret = PCube_setMinPos(device_, module_ids_[i], min_positions_[i]);
        pthread_mutex_unlock(&mutex_);
    }

    if (ret != 0)
    {
        ROS_ERROR_STREAM("Schunk::send_min_positions - Could not communicate with hardware. m5api error code: " << ret);
        return false;
    }
    return true;
}

bool Schunk::send_home_offsets()
{
    int ret = 0;

    for (int i = 0; i < module_count_; i++)
    {
        pthread_mutex_lock(&mutex_);
        ret = PCube_setHomeOffset(device_, module_ids_[i], home_offsets_[i]);
        pthread_mutex_unlock(&mutex_);
    }

    if (ret != 0)
    {
        ROS_ERROR_STREAM("Schunk::send_home_offsets - Could not communicate with hardware. m5api error code: " << ret);
        return false;
    }
    return true;
}

bool Schunk::send_home_velocities()
{
    int ret = 0;

    for (int i = 0; i < module_count_; i++)
    {
        pthread_mutex_lock(&mutex_);
        ret = PCube_setHomeVel(device_, module_ids_[i], home_velocities_[i]);
        pthread_mutex_unlock(&mutex_);
    }

    if (ret != 0)
    {
        ROS_ERROR_STREAM("Schunk::send_home_velocities - Could not communicate with hardware. m5api error code: " << ret);
        return false;
    }
    return true;
}

bool Schunk::send_max_velocities()
{
    int ret = 0;

    for (int i = 0; i < module_count_; i++)
    {
        pthread_mutex_lock(&mutex_);
        ret = PCube_setMaxVel(device_, module_ids_[i], max_velocities_[i]);
        pthread_mutex_unlock(&mutex_);
    }

    if (ret != 0)
    {
        ROS_ERROR_STREAM("Schunk::send_max_velocities - Could not communicate with hardware. m5api error code: " << ret);
        return false;
    }
    return true;
}

bool Schunk::send_max_accelerations()
{
    int ret = 0;

    for (int i = 0; i < module_count_; i++)
    {
        pthread_mutex_lock(&mutex_);
        ret = PCube_setMaxAcc(device_, module_ids_[i], max_accelerations_[i]);
        pthread_mutex_unlock(&mutex_);
    }

    if (ret != 0)
    {
        ROS_ERROR_STREAM("Schunk::send_max_accelerations - Could not communicate with hardware. m5api error code: " << ret);
        return false;
    }
    return true;
}

/******************************************************************************
                               INTERNAL METHODS
 *****************************************************************************/

bool Schunk::OpenDevice()
{
    int ret = 0;

    std::ostringstream InitStr;
    InitStr << can_module_ << ":" << can_device_ << "," << can_baudrate_;

    ROS_INFO_STREAM("=========================================================================== ");
    ROS_INFO_STREAM("Opening CAN device_ with InitString: " << InitStr.str());
    ROS_INFO_STREAM("PCube_openDevice() initialization output: ");

    pthread_mutex_lock(&mutex_);
    ret = PCube_openDevice(&device_, InitStr.str().c_str());
    pthread_mutex_unlock(&mutex_);

    if (ret != 0)
    {
        ROS_ERROR_STREAM("Could not open CAN device " << can_device_ << ", m5api error code: " << ret);
        return false;
    }

    can_device_opened_ = true;

    ROS_INFO_STREAM("CAN device opened successfully.");
    ROS_INFO_STREAM("=========================================================================== ");

    return true;
}

bool Schunk::ProbeModules()
{
    ROS_INFO_STREAM("Probing modules ");

    pthread_mutex_lock(&mutex_);
    int module_count = PCube_getModuleCount(device_);
    pthread_mutex_unlock(&mutex_);

    ROS_INFO_STREAM("Found " << module_count << " modules");

    // Check if the modules are connected
    for (int i = 0; i < module_count_; i++)
    {
        unsigned long serNo;

        pthread_mutex_lock(&mutex_);
        int ret = PCube_getModuleSerialNo(device_, module_ids_[i], &serNo);
        pthread_mutex_unlock(&mutex_);

        if (ret != 0)
        {
            ROS_ERROR_STREAM("Could not find Module with ID " << module_ids_[i] << ", m5api error code: " << ret);
            return false;
        }

        ROS_INFO_STREAM("Found module " << (i+1) << " with Serial Number: " << serNo);
    }

    ROS_INFO_STREAM("=========================================================================== ");

    return true;
}

void Schunk::InitMessages()
{
    param_msg_.MinPos.resize(module_count_);
    param_msg_.MinPosInc.resize(module_count_);
    param_msg_.MaxPos.resize(module_count_);
    param_msg_.MaxPosInc.resize(module_count_);
    param_msg_.MaxVel.resize(module_count_);
    param_msg_.MaxVelInc.resize(module_count_);
    param_msg_.MaxAcc.resize(module_count_);
    param_msg_.MaxAccInc.resize(module_count_);
    param_msg_.HomeOffset.resize(module_count_);
    param_msg_.HomeOffsetInc.resize(module_count_);
    param_msg_.HomeVel.resize(module_count_);
    param_msg_.HomeVelInc.resize(module_count_);
    param_msg_.MaxCur.resize(module_count_);
    param_msg_.MaxDeltaPos.resize(module_count_);
    param_msg_.MaxDeltaPosInc.resize(module_count_);
    param_msg_.A0.resize(module_count_);
    param_msg_.C0.resize(module_count_);
    param_msg_.Damp.resize(module_count_);
    param_msg_.MinLogicVoltage.resize(module_count_);
    param_msg_.MaxLogicVoltage.resize(module_count_);
    param_msg_.MinMotorVoltage.resize(module_count_);
    param_msg_.MaxMotorVoltage.resize(module_count_);
    param_msg_.NominalMotorCurrent.resize(module_count_);
    param_msg_.MaximumMotorCurrent.resize(module_count_);
    param_msg_.LogicUndershootTime.resize(module_count_);
    param_msg_.LogicOvershootTime.resize(module_count_);
    param_msg_.MotorUndershootTime.resize(module_count_);
    param_msg_.MotorOvershootTime.resize(module_count_);
    param_msg_.NomCurOvershootTime.resize(module_count_);
    param_msg_.HMaxCurOvershootTime.resize(module_count_);
    param_msg_.CurrentLimit.resize(module_count_);
    param_msg_.KpPWMLimit.resize(module_count_);
    param_msg_.MaxPWMOutput.resize(module_count_);
    param_msg_.ModuleType.resize(module_count_);
    param_msg_.ModuleVersion.resize(module_count_);
    param_msg_.ModuleSerialNo.resize(module_count_);
    param_msg_.LoadLimit.resize(module_count_);
    param_msg_.MaxLoadGradient.resize(module_count_);
    param_msg_.LoadDeltaTime.resize(module_count_);
    param_msg_.DefCANBaudRate.resize(module_count_);
    param_msg_.DefRSBaudRate.resize(module_count_);
    param_msg_.DefBaudRate.resize(module_count_);
    param_msg_.DefBurnCount.resize(module_count_);
    param_msg_.DefGearRatio.resize(module_count_);
    param_msg_.DefLinearRatio.resize(module_count_);
    param_msg_.DefCurRatio.resize(module_count_);
    param_msg_.DefIncPerTurn.resize(module_count_);
    param_msg_.DefBrakeTimeOut.resize(module_count_);
    param_msg_.DefMinPos.resize(module_count_);
    param_msg_.DefMaxPos.resize(module_count_);
    param_msg_.DefMaxVel.resize(module_count_);
    param_msg_.DefMaxAcc.resize(module_count_);
    param_msg_.DefHomeOffset.resize(module_count_);
    param_msg_.DefHomeVel.resize(module_count_);
    param_msg_.DefMaxCur.resize(module_count_);
    param_msg_.DefMaxDeltaPos.resize(module_count_);
    param_msg_.DefA0.resize(module_count_);
    param_msg_.DefC0.resize(module_count_);
    param_msg_.DefDamp.resize(module_count_);

    joint_msg_.ModuleState.resize(module_count_);
    joint_msg_.Dio.resize(module_count_);
    joint_msg_.Pos.resize(module_count_);
    joint_msg_.PosInc.resize(module_count_);
    joint_msg_.PosCountInc.resize(module_count_);
    joint_msg_.Vel.resize(module_count_);
    joint_msg_.VelInc.resize(module_count_);
    joint_msg_.IPolVel.resize(module_count_);
    joint_msg_.Cur.resize(module_count_);
    joint_msg_.CurInc.resize(module_count_);
    joint_msg_.DeltaPos.resize(module_count_);
    joint_msg_.DeltaPosInc.resize(module_count_);

    elec_msg_.RawMotorCurrent.resize(module_count_);
    elec_msg_.RawMotorSupply.resize(module_count_);
    elec_msg_.RawTemperature.resize(module_count_);
    elec_msg_.RawLogicSupply.resize(module_count_);
    elec_msg_.MotorCurrent.resize(module_count_);
    elec_msg_.MotorSupply.resize(module_count_);
    elec_msg_.Temperature.resize(module_count_);
    elec_msg_.LogicSupply.resize(module_count_);
}

void Schunk::PrintParameters()
{
    // Output of current settings in the terminal
    ROS_INFO_STREAM("can_module: " << can_module_);
    ROS_INFO_STREAM("can_device_: " << can_device_);
    ROS_INFO_STREAM("can_baudrate: " << can_baudrate_);
    ROS_INFO_STREAM("module_count: " << module_count_);

    std::ostringstream* temp = new std::ostringstream(ostringstream::out);
    *temp << "module_names: ";
    for (int i = 0; i < module_count_; i++)
    {
        *temp << module_names_[i] << " ";
    }
    ROS_INFO_STREAM(temp->str());
    delete(temp);


    temp = new std::ostringstream(ostringstream::out);
    *temp << "module_ids: ";
    for (int i = 0; i < module_count_; i++)
    {
        *temp << module_ids_[i] << " ";
    }
    ROS_INFO_STREAM(temp->str());
    delete(temp);

    temp = new std::ostringstream(ostringstream::out);
    *temp << "max_positions: ";
    for (int i = 0; i < module_count_; i++)
    {
        *temp << max_positions_[i] << " ";
    }
    ROS_INFO_STREAM(temp->str());
    delete(temp);

    temp = new std::ostringstream(ostringstream::out);
    *temp << "min_positions: ";
    for (int i = 0; i < module_count_; i++)
    {
        *temp << min_positions_[i] << " ";
    }
    ROS_INFO_STREAM(temp->str());
    delete(temp);

    temp = new std::ostringstream(ostringstream::out);
    *temp << "home_offsets: ";
    for (int i = 0; i < module_count_; i++)
    {
        *temp << home_offsets_[i] << " ";
    }
    ROS_INFO_STREAM(temp->str());
    delete(temp);

    temp = new std::ostringstream(ostringstream::out);
    *temp << "home_velocities: ";
    for (int i = 0; i < module_count_; i++)
    {
        *temp << home_velocities_[i] << " ";
    }
    ROS_INFO_STREAM(temp->str());
    delete(temp);

    temp = new std::ostringstream(ostringstream::out);
    *temp << "max_velocities: ";
    for (int i = 0; i < module_count_; i++)
    {
        *temp << max_velocities_[i] << " ";
    }
    ROS_INFO_STREAM(temp->str());
    delete(temp);

    temp = new std::ostringstream(ostringstream::out);
    *temp << "max_accelerations: ";
    for (int i = 0; i < module_count_; i++)
    {
        *temp << max_accelerations_[i] << " ";
    }
    ROS_INFO_STREAM(temp->str());
    delete(temp);

}
